#include "Separation.h"
#include "../Vectores.h"
#include "../Character.h"
#include "../Obstacle.h"
#include "Obs_Sep.h"
#include "../CApp.h"
#include <cmath>
#include <stdlib.h>
#include <iostream>

Separation::Separation(vector<Character *>* charactrs,vector<Obstacle *>* _obstacle,Character* _player){
  /*
  threshold = 100;
  decayCoefficient = 6;
  maxAcceleration = 1000;
  */
  threshold = 50;
  decayCoefficient = 2;
  maxAcceleration = 10;
  characters =charactrs;
  obstacle = _obstacle;
  player = _player;

}

Steering* Separation::getSteering(){
  Steering* ret = new Steering();
  float direction[3];
  float distance;
  float strength;
  for (unsigned int  i =0 ; i < characters->size();i++){
    if ((*characters)[i] != theCharacter){
      Kinematic *b = (*characters)[i]->getKinematic();
      restarVectores(character->position,b->position,direction);
      distance = length(direction);
      if (distance < threshold){
        strength = min(decayCoefficient / (distance * distance), maxAcceleration);
        normalize(direction);
        multVectorScalar(direction,strength);
        sumarVectores(ret->linear,direction,ret->linear);
      }
      delete(b);
    }
  }
  // Esto es para separation con los enemigos
  for (unsigned int  i =0 ; i < enemySep->size();i++){
    if ((*enemySep)[i] != theCharacter){
      Kinematic *b = (*enemySep)[i]->getKinematic();
      restarVectores(character->position,b->position,direction);
      distance = length(direction);
      if (distance < threshold){
        strength = min(decayCoefficient / (distance * distance), maxAcceleration);
        normalize(direction);
        multVectorScalar(direction,strength);
        sumarVectores(ret->linear,direction,ret->linear);
      }
      delete(b);
    }
  }
  restarVectores(character->position,target->position,direction);
  distance = length(direction);
  if (distance < threshold){
    strength = min(decayCoefficient / (distance * distance), maxAcceleration);
    normalize(direction);
    multVectorScalar(direction,strength);
    sumarVectores(ret->linear,direction,ret->linear);
  }
/*

  for ( i =0 ; i < obstacle->size();i++){
    restarVectores(character->position,(*obstacle)[i]->position,direction);
    distance = length(direction);
    if (distance < threshold){
      strength = min(decayCoefficient / (distance * distance), maxAcceleration);
      normalize(direction);
      multVectorScalar(direction,strength);
      sumarVectores(ret->linear,direction,ret->linear);
    }
  }


  //DEBUG
//    cout << "getSteering Separation" << "ret->linear " << ret->linear[0] << "," <<ret->linear[1] << "angular" << ret->angular <<endl;
*/
  return ret;
}


